#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <numeric>
#include <algorithm>
#include <vector>
#include <string>
#include <cassert>
#include "Core/Interval.h"
#include "Core/numlib.h"
#include "Core/TensorExpr.h"
#include "Embedded/PLG.h"

using i2 = Vec<int, 2>;
using r2 = Vec<Real, 2>;
using i3 = Vec<int, 3>;
using r3 = Vec<Real, 3>;

const char *prefix[] = {"cv", "cc"};

template <int _n>
void test2D(i2 q, r2 center, r2 radius, Real theta, bool concave, const std::string &type, int maxNumSol)
{
  std::cout << "type = " << type << ", D = 2, n = " << _n << ", q = " << q
            << ", center = " << center << ", radius = " << radius << ", theta = " << theta
            << ", concave = " << std::boolalpha << concave << std::endl;
  // 1. Define the PLG problem.
  const int n = _n;
  const int D = 2;
  Tensor<int, D> K(Box<D>(0, n));
  K = 0;
  if(type == "ellipse") {
    for (int j = 0; j <= n; ++j) {
      for (int i = 0; i <= n; ++i) {
        r2 z = (i2 {i, j} - center) / radius;
        if(dot(z, z) <= 1.0)
          K(i, j) = 1;
      }
    }
    if (!concave)
      K = -K + 1;
  } else if(type == "box") {
    Interval<D> kbox(0.0, radius);
    r2 ax[] { {cos(theta), sin(theta)}, {-sin(theta), cos(theta)} };
    for (int j = 0; j <= n; ++j) {
      for (int i = 0; i <= n; ++i) {
        r2 x {1.0*i - center[0], 1.0*j - center[1]};
        r2 a {dot(x,ax[0]), dot(x,ax[1])};
        if(kbox.contain(a, 1e-12))
          K(i, j) = 1;
      }
    }
  }
  dbgcout3 << "K = " << K << std::endl;
  // 2. Build the search order.
  PLG<D,n>::initialize();
  bool earlyReject;
  Real minCpt;
  auto searchOrder = PLG<D,n>::generateTestOrder(K, q * 1.0, PLG<D,n>::TestOrder::oneNormFirst, earlyReject);
  // 3. Run the PLG algorithm.
  auto sol = PLG<D,n>::generate(K, q, q * 1.0, searchOrder, minCpt, maxNumSol);
  assert(minCpt > 0.0);
//  std::cout << "Found " << solutions.size() << " solutions. Will output "
//            << maxOutputCases << " of them. " << std::endl;
  // 4.
  std::ostringstream oss;
  oss << "output/S-" << D << "-" << n << "-";
  if(type == "ellipse")
    oss << prefix[concave];
  else if(type == "box")
    oss << "bx";
  oss << ".txt";
  std::ofstream outfile(oss.str());
  outfile << type[0] << " "
          << D << " " << n << " "
          << q[0] << " " << q[1] << " "
          << center[0] << " " << center[1] << " "
          << radius[0] << " " << radius[1] << " "
          << theta << "\n";
  outfile << "1 \n";
  auto vi = PLG<D,n>::form(sol);
  for(auto idx : vi)
    outfile << idx[0] << " " << idx[1] << " ";
  outfile << "\n";
}

template <int _n>
void test3D(i3 q, r3 center, r3 radius, Real theta, bool concave, const std::string &type, int maxNumSol)
{
  std::cout << "type = " << type << ", D = 3, n = " << _n << ", q = " << q
            << ", center = " << center << ", radius = " << radius << ", theta = " << theta
            << ", concave = " << std::boolalpha << concave << std::endl;
  // 1. Define the PLG problem.
  const int n = _n;
  const int D = 3;
  Tensor<int, D> K(Box<D>(0, n));
  K = 0;
  if(type == "ellipsoid") {
    for(int k = 0; k <= n; ++k) {
      for (int j = 0; j <= n; ++j) {
        for (int i = 0; i <= n; ++i) {
          r3 z = (i3 {i, j, k} - center) / radius;
          if (dot(z, z) <= 1.0)
            K(i, j, k) = 1;
        }
      }
    }
    if (!concave)
      K = -K + 1;
  } else if(type == "box") {
    Interval<D> kbox(0.0, radius);
    r3 ax[3];
    ax[0] = {cos(theta)*cos(theta) - sin(theta)*cos(theta)*sin(theta), sin(theta)*cos(theta) + cos(theta)*cos(theta)*sin(theta), sin(theta)*sin(theta)};
    ax[2] = {sin(theta)*sin(theta), -cos(theta)*sin(theta), cos(theta)};
    ax[1] = cross(ax[2], ax[0]);
    dbgcout2 << "norm of ax[1] = " << norm(ax[1], 2) << std::endl;
    for(int k = 0; k <= n; ++k) {
      for (int j = 0; j <= n; ++j) {
        for (int i = 0; i <= n; ++i) {
          r3 x {1.0 * i - center[0], 1.0 * j - center[1], 1.0 * k - center[2]};
          r3 a {dot(x, ax[0]), dot(x, ax[1]), dot(x, ax[2])};
          if (kbox.contain(a, 1e-12))
            K(i, j, k) = 1;
        }
      }
    }
  }
  dbgcout3 << "K = " << K << std::endl;
  // 2. Build the search order.
  PLG<D,n>::initialize();
  bool earlyReject;
  Real minCpt;
  auto searchOrder = PLG<D,n>::generateTestOrder(K, q * 1.0, PLG<D,n>::TestOrder::oneNormFirst, earlyReject);
  // 3. Run the PLG algorithm.
  auto sol = PLG<D,n>::generate(K, q, q * 1.0, searchOrder, minCpt, maxNumSol);
  assert(minCpt > 0.0);
//  std::cout << "Found " << solutions.size() << " solutions. Will output at most "
//            << maxOutputCases << " of them. " << std::endl;
  // 4.
  std::ostringstream oss;
  oss << "output/S-" << D << "-" << n << "-";
  if(type == "ellipsoid")
    oss << prefix[concave];
  else if(type == "box")
    oss << "bx";
  oss << ".txt";
  std::ofstream outfile(oss.str());
  outfile << type[0] << " "
          << D << " " << n << " "
          << q[0] << " " << q[1] << " " << q[2] << " "
          << center[0] << " " << center[1] << " " << center[2] << " "
          << radius[0] << " " << radius[1] << " " << radius[2] << " "
          << theta << "\n";
//  int numOutputCases = std::min(maxOutputCases, (int)(solutions.size()));
  outfile << "1 \n";
  auto vi = PLG<D,n>::form(sol);
  for(auto idx : vi)
    outfile << idx[0] << " " << idx[1] << " " << idx[2] << " ";
  outfile << "\n";
}

int main(int argc, char *argv[])
{
  reset_dbglevel(1);
  bool t2D = true;
  bool t3D = true;
  int maxNumSol = 1;
  if(t2D) {
    // n == 3
    test2D<3>({2, 2}, {-10.88, -6.88}, {16.0, 12.0}, 0.0, false, "ellipse", maxNumSol);
    test2D<3>({2, 2}, {-8.4, -6.28}, {16.0, 12.0}, 0.0, true, "ellipse", maxNumSol);
    test2D<3>({1, 0}, {0.8, -0.32}, {10.0, 10.0}, M_PI / 6, false, "box", maxNumSol);
    std::cout << std::endl;
    // n == 4
    test2D<4>({2, 1}, {-10.88, -6.88}, {16.0, 12.0}, 0.0, false, "ellipse", maxNumSol);
    test2D<4>({3, 3}, {-8.4, -4.48}, {16.0, 12.0}, 0.0, true, "ellipse", maxNumSol);
    test2D<4>({1, 0}, {0.8, -0.32}, {10.0, 10.0}, M_PI / 6, false, "box", maxNumSol);
    std::cout << std::endl;
    // n == 5
    test2D<5>({2, 2}, {-10.88, -6.88}, {16.0, 12.0}, 0.0, false, "ellipse", maxNumSol);
    test2D<5>({3, 3}, {-8.4, -4.48}, {16.0, 12.0}, 0.0, true, "ellipse", maxNumSol);
    test2D<5>({1, 0}, {0.8, -0.32}, {10.0, 10.0}, M_PI / 6, false, "box", maxNumSol);
    std::cout << std::endl;
  }
  if(t3D) {
    // n == 3
    test3D<3>({ 1, 1, 1 }, {12.0, 9.0, -3.0}, {15.0, 12.0, 9.0}, 0.0, false, "ellipsoid", maxNumSol);
    test3D<3>({ 2, 2, 2 }, {0.0, 0.0, 0.0}, {3.6, 3.6, 3.6}, 0.0, true, "ellipsoid", maxNumSol);
    test3D<3>({ 1, 1, 0 }, {1.0, 1.0, -0.5}, {10.0, 10.0, 10.0}, M_PI / 4, false, "box", maxNumSol);
    std::cout << std::endl;
    // n == 4
    test3D<4>({ 2, 2, 1 }, {16.0, 12.0, -4.0}, {20.0, 16.0, 12.0}, 0.0, false, "ellipsoid", maxNumSol);
    test3D<4>({ 2, 2, 3 }, {0.0, 0.0, 0.0}, {4.8, 4.8, 4.8}, 0.0, true, "ellipsoid", maxNumSol);
    test3D<4>({ 2, 2, 0 }, {2.0, 2.0, -0.5}, {10.0, 10.0, 10.0}, M_PI / 4, false, "box", maxNumSol);
    std::cout << std::endl;
  }
  return 0;
}
